#include "MaKI.h"


#include <vector>
#include <deque>
#include <algorithm>

#include <stddef.h>
#include <stdio.h>
#include <sys/types.h>
#include <time.h> //needed for random seed

#ifndef WIN32
	#include <dirent.h>
	#include <fnmatch.h>
#endif

#include "stdafx.h"
#include "featureDetection/FeatureDetector.h"
#include "featureDetection/SimpleFeatureDetector.h"
#include "featureDetection/SURF.h"
#include "correlation/VarianceCorrelator.h"
#include "gui/GUI.h"
#include "utils/Settings.h"
#include "output/DataWriter.h"


MaKI::MaKI()
{
	
	this->gui = new GUI();
	
	
	/* we could make a nifty factory in the FeatureDetector class, but there will be
	 * only 2 or 3 possible types, so this is much simpler and works as well */
	switch (Settings::featureDetectorType) {
		case FeatureDetector::SIMPLE:
			this->featureDetector = new SimpleFeatureDetector();
			break;
		case FeatureDetector::SURF:
			this->featureDetector = new SURF();
			break;
		default:
			break;
	}
	
	this->correlator = new VarianceCorrelator();
	this->dataWriter = new DataWriter();
}

MaKI::~MaKI()
{
}






/**
 * Retrieve the file names in a given directory that meet a given pattern.
 * 
 * @param basePath The base path of the directory to read
 * @param pattern The pattern the file names have to match to be returned (e.g. *.tif);
 * @param fileNames Pre-allocated vector of strings. File names will be placed in here.
 * @param returnFullPaths States wether fileNames will contain full paths or just the file names.
 */
void MaKI::getFileNames(string basePath, string pattern, vector<string>& fileNames, bool returnFullPaths){

	///TODO use boost::filesystem for file access
	
	
	/* The following blocks (switched by preprocessor directive) are logically identical save
	 * for the intruction used for reading the directories.
	 * This leads to some copy-and-paste code, especially inside the inner loop, but breaking this
	 * into two separate large blocks with some identical code improves readability to making an
	 * #ifdef statement araound each block/line that is different.
	 */
	
#ifndef WIN32
	DIR *dp;
	struct dirent *ep;
	
	string currentFileName;
	

	//if there are files in the directory...

	dp = opendir (basePath.c_str());
    if (dp != NULL) {		
		while ((ep = readdir (dp))) {
			if (fnmatch(pattern.c_str(), ep->d_name, FNM_PATHNAME|FNM_PERIOD) != FNM_NOMATCH) 	{
				
				//give path and filename to loadImage()
				if (returnFullPaths) {
					currentFileName += basePath;
					currentFileName += "/";
				}
			
				currentFileName += ep->d_name;


				
				fileNames.push_back(currentFileName);
				
				currentFileName="";
			}

		}

		(void) closedir (dp);
	} else {
		Output::message("Could not open dir: %s", Output::CRITICAL, basePath.c_str());	
	}
		
#else
	
	struct _finddata_t fd;
	intptr_t hFile;
	bool done;
	string fname;
	fname += basePath;
	fname += pattern;
	
	string currentFileName;
	

	//if there are files in the directory...
	if ( (hFile = _findfirst(fname.c_str(), &fd )) != -1)
	{
		
		done = false; 
		//...and while there are files left, do...
		while (!done)
		{
			if (!(fd.name[0] == '.' && (fd.name[1] == 0 || fd.name[1] == '.' && fd.name[2] == 0)))
			{
				//give path and filename to loadImage()
				if (returnFullPaths) {
					currentFileName += basePath;
					currentFileName += "\\";
				}
				
				currentFileName += fd.name;

				/*char * c2 = new char[filePath.length()+1];
				strcpy(c2,filePath.c_str());*/
				
				fileNames.push_back(currentFileName);
				currentFileName="";
			}
			done = (_findnext(hFile, &fd) != 0); //Yes, this could be assigned directly, but would lessen readability
		}
	}		
#endif
	
	sort(fileNames.begin(), fileNames.end());
	
}







//************************************
// Method:    	loadImage
// FullName:  	loadImage
// Access:    	public 
// Returns:   	void
// Parameter: 	std::string filename
// Description:	Loading image with given filename
//************************************
bool MaKI::loadImage(std::string filename, IplImage* &image)
{
	Output::message("Loading image %s", Output::VERBOSE, filename.c_str());
	
	unsigned int width=0,height=0;
	
	bool success;
	
	//open image with LibTiff
	TIFF* tif= TIFFOpen(filename.c_str(), "r"); 

	//IplImage *image;
	//cvReleaseImage(&image);
	size_t npixels;
	uint32 *raster;
	std::string datname;
	int datnamePos1=0, datnamePos2=0;

	//if file can be opened
	if (tif)
	{
		//get width & height
		TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, &width);           
		TIFFGetField(tif, TIFFTAG_IMAGELENGTH, &height);  

		//get image data.
		npixels=width*height;
		raster = (uint32*) _TIFFmalloc(npixels * sizeof (uint32));

		//OpenCV can't handle TIFF* => copy from raster to IplImage *image
		if (raster != NULL)
		{
			if (TIFFReadRGBAImage(tif, width, height, raster, 0))
			{
				image = cvCreateImage( cvSize(width,height), IPL_DEPTH_8U, 1 );

				//give data from raster to *image step by step
				for (unsigned int i=0; i<height; i++)
				{
					for (unsigned int j=0; j<width; j++)
					{
						(((uchar*)(image->imageData + image->widthStep*((height-i-1)))))[j * image->nChannels] = (uchar)raster[i*width + j];
					}
				}

				for(int i=filename.length()-1; i>=0; i--)
				{
					if(filename[i]=='.')
						datnamePos2=i;
					else if (filename[i]=='\\')
					{
						datnamePos1=i+1;
						i=0;
					}
				}
				datname=filename.substr(datnamePos1,datnamePos2-datnamePos1);

			}
			_TIFFfree(raster);
			success = true;
		} else {
			
			cout<<"ERROR: Raster = NULL"<<endl;
			
			success = false;
		}
		
		
		TIFFClose(tif);
		
	} else {
		success = false;
	}
	
	return success;

		
}









void MaKI::processImages(string filePath, vector<string> fileNames){
	IplImage* imageToLoad;
	CvSeq* featurePoints;
	CvMemStorage* storageFeatures;
	
	
	std::deque<IplImage*> images;
	std::deque<CvSize> imageSizes;
	std::deque<CvSeq*> features;
	std::deque<CvMemStorage*> memStoragesFeatures; 
	std::deque<CvSeq*> featurePairs;
	std::deque<CvMemStorage*> memStoragesPairs;
	
	CvSeq* currentPairs;
	CvMat* homography;
	
	bool imageLoaded;
	
	std::vector<std::string> fullFileNames;
	std::ostringstream fullFileName;
	for (unsigned int i = 0; i < fileNames.size(); ++i) {
		fullFileName.str("");
		fullFileName << filePath << fileNames[i];
		fullFileNames.push_back(fullFileName.str());
	}
	
	
	imageLoaded = loadImage(fullFileNames[0], imageToLoad);
	if (imageLoaded){
		images.push_back(imageToLoad);
		imageSizes.push_back(cvGetSize(images.back()));
		
	} else {
		Output::message("Could not load image %s", Output::CRITICAL, fullFileNames[0].c_str());
		return;
	}
	
	imageLoaded = loadImage(fullFileNames[1], imageToLoad);
	if (imageLoaded){
		images.push_back(imageToLoad);
		imageSizes.push_back(cvGetSize(images.back()));
		
	} else {
		Output::message("Could not load image %s", Output::CRITICAL, fullFileNames[1].c_str());
		return;
	}
	
	
	
	
	/* NOTE:
	 * searching for features in a specified ROI only is not feasible if we aim to reuse previously
	 * found features, as the overlapping regions between images will vary.
	 * Thus, we detect features in the whole image and filter when establishing correspondence. */
	storageFeatures = cvCreateMemStorage(Settings::maxFeaturePoints * sizeof(FeatureDetector::Feature));
	featurePoints = cvCreateSeq(0, sizeof(CvSeq), sizeof(FeatureDetector::Feature),storageFeatures);

	
	
	
	Output::message("Detecting features for image %s", Output::VERBOSE, fullFileNames[0].c_str());
	
	this->featureDetector->detectFeaturePoints(images[0],featurePoints);
	//memStoragesFeatures.push_back(cvCreateMemStorage(featurePoints->total * sizeof(FeatureDetector::Feature)));
	memStoragesFeatures.push_back(cvCreateMemStorage(0));
	features.push_back(cvCloneSeq(featurePoints, memStoragesFeatures.back()));
	
	this->gui->showFeatures(images[0], featurePoints, fullFileNames[0]);
	
	
	Output::message("Detecting features for image %s", Output::VERBOSE, fullFileNames[1].c_str());
	
	this->featureDetector->detectFeaturePoints(images[1],featurePoints);
	//memStoragesFeatures.push_back(cvCreateMemStorage(featurePoints->total * sizeof(FeatureDetector::Feature)));
	memStoragesFeatures.push_back(cvCreateMemStorage(0));
	features.push_back(cvCloneSeq(featurePoints, memStoragesFeatures.back()));
	
	this->gui->showFeatures(images[1], featurePoints, fullFileNames[1]);
	
	
	
	
	Output::message("Correlating features for images 0 and 1", Output::VERBOSE);
	
	currentPairs = this->correlator->correlate(images[0], images[1], features[0], features[1], Settings::maxImageOverlap);
	Output::message("Feature pairs: %d", Output::DEBUG, currentPairs->total);
	this->gui->showCorrelation(images[0], images[1], currentPairs, "Feature_matches", 0, 1);
	

	
	
	Output::message("Establishing homography", Output::VERBOSE);
	
	homography = this->findHomography(currentPairs); //currentPairs will be edited during this!
	if (homography != NULL){
		Output::message("Feature pairs after filtration: %d", Output::DEBUG, currentPairs->total);
		this->gui->showCorrelation(images[0], images[1], currentPairs, "Feature_matches_refined", 0, 1);
		
		this->testHomography(images[0], images[1], currentPairs, homography, 0, 1);
	}
	
	this->adjustPairIDs(currentPairs, 0, 1);
	
	memStoragesPairs.push_back(cvCreateMemStorage(currentPairs->total * sizeof(FeatureDetector::FeaturePair)));
	featurePairs.push_back(cvCloneSeq(currentPairs, memStoragesPairs.back()));
	
	//findFundamentalMatrix(currentPairs);
	
	

	for (unsigned int i = 2; i < fullFileNames.size(); ++i){
		
		imageLoaded = loadImage(fullFileNames[i], imageToLoad);
		if (imageLoaded){
			images.push_back(imageToLoad);
			imageSizes.push_back(cvGetSize(images.back()));
			
		} else {
			Output::message("Could not load image %s", Output::CRITICAL, fullFileNames[i].c_str());
			return;
		}
		
		Output::message("Detecting features for image %s", Output::VERBOSE, fullFileNames[i].c_str());
		this->featureDetector->detectFeaturePoints(images[2],featurePoints);
//		memStoragesFeatures.push_back(cvCreateMemStorage(featurePoints->total * sizeof(FeatureDetector::Feature)));
		memStoragesFeatures.push_back(cvCreateMemStorage(0));
		features.push_back(cvCloneSeq(featurePoints, memStoragesFeatures.back()));
		this->gui->showFeatures(images[1], featurePoints, fullFileNames[1]);
		
		
		/* match feature points between images 1/3 and 2/3 (1/2 has already been done) */
		Output::message("Correlating features for images %d and %d", Output::VERBOSE, i-2, i);
		
		currentPairs = this->correlator->correlate(images[0], images[2], features[0], features[2], (Settings::maxImageOverlap * 2) -1.0);
		Output::message("Feature pairs: %d", Output::DEBUG, currentPairs->total);
		this->gui->showCorrelation(images[0], images[2], currentPairs, "Feature_matches", i-2, i);
		
		
		Output::message("Establishing homography", Output::VERBOSE);
		
		homography = this->findHomography(currentPairs); //currentPairs will be edited during this!
		if(homography != NULL) {
			Output::message("Feature pairs after filtration: %d", Output::DEBUG, currentPairs->total);
			this->gui->showCorrelation(images[0], images[2], currentPairs, "Feature_matches_refined", i-2, i);
			
			this->testHomography(images[0], images[2], currentPairs, homography, i-2, i);
		}
		
		this->adjustPairIDs(currentPairs, i-2, i);
		
		memStoragesPairs.push_back(cvCreateMemStorage(currentPairs->total * sizeof(FeatureDetector::FeaturePair)));
		featurePairs.push_back(cvCloneSeq(currentPairs, memStoragesPairs.back()));
		
		
		
		///TODO get and store fundemental matrix (RANSAC)
		
		Output::message("Correlating features for images %d and %d", Output::VERBOSE, i-1, i);
		
		currentPairs = this->correlator->correlate(images[1], images[2], features[1], features[2], Settings::maxImageOverlap);
		Output::message("Feature pairs: %d", Output::DEBUG, currentPairs->total);
		this->gui->showCorrelation(images[1], images[2], currentPairs, "Feature_matches", i-1, i);
		
		
		Output::message("Establishing homography", Output::VERBOSE);
		
		homography = this->findHomography(currentPairs); //currentPairs will be edited during this!
		if(homography != NULL) {
			Output::message("Feature pairs after filtration: %d", Output::DEBUG, currentPairs->total);
			this->gui->showCorrelation(images[1], images[2], currentPairs, "Feature_matches_refined", i-1, i);
			
			this->testHomography(images[1], images[2], currentPairs, homography, i-1, i);
		}
			
		this->adjustPairIDs(currentPairs, i-1, i);
		
		//memStoragesPairs.push_back(cvCreateMemStorage(currentPairs->total * sizeof(FeatureDetector::FeaturePair)));
		memStoragesPairs.push_back(cvCreateMemStorage(0));
		featurePairs.push_back(cvCloneSeq(currentPairs, memStoragesPairs.back()));
		
		
		///TODO get and store fundemental matrix (RANSAC)
		
		
		/* delete and release the first image */
		cvReleaseImage(&(images.front()));
		cvReleaseMemStorage(&(memStoragesFeatures.front()));
		images.pop_front();
		memStoragesFeatures.pop_front();
		features.pop_front();

	}
	
	Output::message("Writing data to file", Output::VERBOSE);
	this->dataWriter->writeToFile(fullFileNames, featurePairs, imageSizes);
	
	cvReleaseImage(&imageToLoad);
	cvReleaseMemStorage(&storageFeatures);

	
}






CvMat* MaKI::findHomography(CvSeq* seqMatches){

	
	if (seqMatches->total < Settings::ransacMinNumOfSupporters) {
		Output::message("Not enough pairs for finding homography (given: %d, needed: %d)", Output::IMPORTANT, seqMatches->total, Settings::ransacMinNumOfSupporters);
		///TODO clear sequence?
		return NULL;
	}

	CvMat *homography;
	CvMat *homographyCandidate;
	CvMat *homographyInv;
	CvMat *homographyVector;
	
	CvMat *BMaybeInliers;
	CvMat *AMaybeInliers;
	
	CvMat *calculatedRightPositions;
	CvMat *leftPositions;
	CvRNG rngState; /* state of random number generator */
	CvSeq* swapTmp;
	FeatureDetector::FeaturePair* currentMatch;
	
	float error;
	
	CvMemStorage* storageMaybeInliers;
	CvMemStorage* storageCurrentInliers;
	CvMemStorage* storageMostInliers;
	CvMemStorage* storageFinalMatches;
	CvSeq* maybeInliers;
	CvSeq* currentInliers;
	CvSeq* mostInliers;
	CvSeq* finalMatches;

	
	storageFinalMatches = cvCreateMemStorage(Settings::maxFeaturePoints * sizeof(FeatureDetector::FeaturePair));
	finalMatches = cvCreateSeq(0, sizeof(CvSeq), sizeof(FeatureDetector::FeaturePair), storageFinalMatches);

	homographyInv = cvCreateMat(3, 3, CV_32F);
	
	homography = cvCreateMat(3, 3, CV_32F);
	homographyCandidate = cvCreateMat(3, 3, CV_32F);
	homographyVector = cvCreateMat(8, 1, CV_32F);
	BMaybeInliers = cvCreateMat(2 * Settings::ransacNumOfMaybeInliers, 1, CV_32F);
	AMaybeInliers = cvCreateMat(2 * Settings::ransacNumOfMaybeInliers, 8, CV_32F);
	calculatedRightPositions = cvCreateMat(3, 1, CV_32F);
	leftPositions = cvCreateMat(3, 1, CV_32F);
	rngState = cvRNG(time(NULL));
	
	storageMaybeInliers = cvCreateMemStorage(Settings::ransacNumOfMaybeInliers   * sizeof(FeatureDetector::FeaturePair));
	storageCurrentInliers  = cvCreateMemStorage(Settings::maxFeaturePoints * sizeof(FeatureDetector::FeaturePair));
	storageMostInliers  = cvCreateMemStorage(Settings::maxFeaturePoints * sizeof(FeatureDetector::FeaturePair));
	maybeInliers = cvCreateSeq(0, sizeof(CvSeq), sizeof(FeatureDetector::FeaturePair), storageMaybeInliers);
	currentInliers  = cvCreateSeq(0, sizeof(CvSeq), sizeof(FeatureDetector::FeaturePair), storageCurrentInliers);
	mostInliers  = cvCreateSeq(0, sizeof(CvSeq), sizeof(FeatureDetector::FeaturePair), storageMostInliers);
	
	

	float meanError;
	float minError;
	
	meanError = 0.0;
	minError = 32768.0;
	

	// iterate the RANSAC algorithm a fixed time of iterations or stop when
	// one Match
	cvClearSeq(finalMatches);
	cvClearSeq(currentInliers);
	cvClearSeq(mostInliers);
	
	
	
	for (int i = 0; i < Settings::ransacNumOfIterations 
			&& (mostInliers->total < seqMatches->total * Settings::ransacStopIterationAtPercentageOfMatches); i++) {
		// get random matches and assume them to be inliers
		cvSetZero(AMaybeInliers);
		cvSetZero(BMaybeInliers);
		
		
		for (int j = 0; j < Settings::ransacNumOfMaybeInliers; j++) {
			// pick a random feature match
			currentMatch = (FeatureDetector::FeaturePair*)cvGetSeqElem(seqMatches, cvRandInt(&rngState) % seqMatches->total);
			cvSeqPush(maybeInliers, currentMatch);
			
			// initiate the A and B matrix for the DLT algorithm .. we set z, z' and h9 = 1
			// A matrix first row
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2, 3) = -1 * currentMatch->left.x;
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2, 4) = -1 * currentMatch->left.y;
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2, 5) = -1;
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2, 6) = currentMatch->right.y * currentMatch->left.x;
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2, 7) = currentMatch->right.y * currentMatch->left.y;
			// A matrix second row
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2 + 1, 0) = currentMatch->left.x;
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2 + 1, 1) = currentMatch->left.y;
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2 + 1, 2) = 1;
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2 + 1, 6) = -1 * currentMatch->right.x * currentMatch->left.x;
			CV_MAT_ELEM(*AMaybeInliers, float, j * 2 + 1, 7) = -1 * currentMatch->right.x * currentMatch->left.y;
			// B matrix
			CV_MAT_ELEM(*BMaybeInliers, float, j * 2, 0) = -1 * currentMatch->right.y;
			CV_MAT_ELEM(*BMaybeInliers, float, j * 2 + 1, 0) = currentMatch->right.x;
		}
		
		// compute the homography H out from the random inliers
		// we get an exact, but biased solution
		cvSolve(AMaybeInliers, BMaybeInliers, homographyVector);
		CV_MAT_ELEM(*homographyCandidate, float, 2, 2) = 1;
		for (int j = 0; j < 8; j++) {
			CV_MAT_ELEM(*homographyCandidate, float, j / 3, j % 3) = CV_MAT_ELEM(*homographyVector, float, j, 0);
		}
				
		// try to find matches that support the model given the random inliers
		cvClearSeq(currentInliers);
		meanError = 0.0;
		for (int j = 0; j < seqMatches->total; j++) {
			currentMatch = (FeatureDetector::FeaturePair*)cvGetSeqElem(seqMatches, j);
			
			// put the values from the position in the last frame in an vector
			CV_MAT_ELEM(*leftPositions, float, 0, 0) = currentMatch->left.x;
			CV_MAT_ELEM(*leftPositions, float, 1, 0) = currentMatch->left.y;
			CV_MAT_ELEM(*leftPositions, float, 2, 0) = 1;
			
			// compute feature position in the new frame using the homography
			// given by the random inliers
			cvMatMul(homographyCandidate, leftPositions, calculatedRightPositions);
			
			// get the error
			//printf("Last:(%.2f, %.2f)  Now:(%.2f, %.2f)  H*Last:(%.2f, %.2f, %.2f)\n", matchPtr->left.x, matchPtr->left.y, matchPtr->right.x, matchPtr->right.y, CV_MAT_ELEM(*X, float, 0, 0), CV_MAT_ELEM(*X, float, 1, 0), CV_MAT_ELEM(*X, float, 2, 0));
			error = sqrt(  powf(CV_MAT_ELEM(*calculatedRightPositions, float, 0, 0) / CV_MAT_ELEM(*calculatedRightPositions, float, 2, 0) - currentMatch->right.x, 2.0f)
					     + powf(CV_MAT_ELEM(*calculatedRightPositions, float, 1, 0) / CV_MAT_ELEM(*calculatedRightPositions, float, 2, 0) - currentMatch->right.y, 2.0f));
			
			//meanError += error;
			
			// if the error is below our threshold we will accept this match
			if (error < Settings::ransacInliersToleratedError) {
				cvSeqPush(currentInliers, currentMatch);
				meanError += error;
				//Debug::message("Accepted pair %d, error: %f", Debug::DEBUG, i, tmp);
			} else {
				//Debug::message("Rejecting Pair %d (%f %f) -> (%f %f), error: %f", Debug::DEBUG, i, matchPtr->left.x, matchPtr->left.y, matchPtr->right.x, matchPtr->right.y, tmp);
			}
			
		}
		
		meanError /= currentInliers->total;
		//Debug::message("Inliers: %d, Mean error: %f", Debug::DEBUG, currentInliers->total, meanError);
		
		// if we have a stronger support for this random inliers than before we take this set
		if (currentInliers->total > mostInliers->total) {
			// swap seqAlsoInliers with seqMostInliers
			swapTmp        = currentInliers;
			currentInliers = mostInliers;
			mostInliers    = swapTmp;
			minError 	   = meanError;
			///TODO clear currentInliers
		} else if(currentInliers->total == mostInliers->total && meanError < minError) {
			swapTmp        = currentInliers;
			currentInliers = mostInliers;
			mostInliers    = swapTmp;
			minError 	   = meanError;
		}
		//Debug::seperator();
	}
	
	Output::message("Most inliers: %d, mean error: %f", Output::DEBUG, mostInliers->total, minError);
	
	//return homographyCandidate;
	
	
	
	
	
	cvReleaseMat(&AMaybeInliers);
	cvReleaseMat(&BMaybeInliers);
	cvReleaseMat(&homographyCandidate);
	
	// we need at least a set of 4 inliers
	if (mostInliers->total < Settings::ransacMinNumOfSupporters) {
		Output::message("RANSAC failed! Couldn't find enough supporting points for a homography (minimum:%d, points found:%d).", 
				Output::CRITICAL, Settings::ransacMinNumOfSupporters, mostInliers->total);
		return NULL;
	}
	

	
	CvMat* leftInliers;
	CvMat* rightInliers;
	
	leftInliers  = cvCreateMat(2, mostInliers->total, CV_32F);
	rightInliers = cvCreateMat(2, mostInliers->total, CV_32F);
	homography = cvCreateMat(3, 3, CV_32F);

	// initiate matrices A and B		
	for (int i = 0; i < mostInliers->total; i++) {
		currentMatch = (FeatureDetector::FeaturePair*)cvGetSeqElem(mostInliers, i);
		cvSeqPush(finalMatches, currentMatch);
	
		
		CV_MAT_ELEM(*leftInliers, float, 0, i) = currentMatch->left.x;
		CV_MAT_ELEM(*leftInliers, float, 1, i) = currentMatch->left.y;
		
		CV_MAT_ELEM(*rightInliers, float, 0, i) = currentMatch->right.x;
		CV_MAT_ELEM(*rightInliers, float, 1, i) = currentMatch->right.y;
		
		
	}
		
	
	
	cvFindHomography(leftInliers, rightInliers, homography);
	
	cvReleaseMat(&leftInliers);
	cvReleaseMat(&rightInliers);
	
	
	Output::message("Rejected pairs: %d", Output::DEBUG, seqMatches->total - finalMatches->total);
	
	/* copy finalMatches to input sequence */
	cvClearSeq(seqMatches);
	for (int i = 0; i < finalMatches->total; ++i) {
		cvSeqPush(seqMatches, cvGetSeqElem(finalMatches, i));
	}

	
	
	
	
	
	
	
	
	
	cvClearSeq(finalMatches);
	cvClearSeq(currentInliers);
	cvClearSeq(maybeInliers);
	cvClearSeq(mostInliers);

	cvReleaseMemStorage(&storageFinalMatches);
	cvReleaseMemStorage(&storageCurrentInliers);
	cvReleaseMemStorage(&storageMaybeInliers);
	cvReleaseMemStorage(&storageMostInliers);
	
	return homography;
	
}



void MaKI::testHomography(IplImage* image1, IplImage* image2, CvSeq* pairs, CvMat* homography, int leftIndex, int rightIndex){
	
	/* ================================================================ */
	/* ======================= HOMOGRAPHY TEST ======================== */
	/* ================================================================ */
	
		CvMat* testPosition;
		CvMat* testResult;
		testPosition = cvCreateMat(3, 1, CV_32F);
		testResult   = cvCreateMat(3, 1, CV_32F);
		
		CvSeq* testResults;
		CvMemStorage* testResultsStorage;
		CvRNG rngState;
		
		rngState = cvRNG(time(NULL));
		
		testResultsStorage = cvCreateMemStorage();
		testResults = cvCreateSeq(0, sizeof(CvSeq), sizeof(FeatureDetector::FeaturePair), testResultsStorage);
		
		FeatureDetector::Feature leftFeature;
		FeatureDetector::Feature rightFeature;
		FeatureDetector::FeaturePair testPair;
		
		int testX;
		int testY;
		int resultX;
		int resultY;
		
		for (int i = 0; i < 30; ++i) {
			
			//cvSetZero(&testPosition);
			//cvSetZero(&testResult);
			
			testX = (cvRandInt(&rngState) % image1->width/2) + image1->width/2;
			testY = cvRandInt(&rngState) % image1->height;
			
			leftFeature.x = testX;
			leftFeature.y = testY;
			
			CV_MAT_ELEM(*testPosition, float, 0, 0) = (float)testX;
			CV_MAT_ELEM(*testPosition, float, 1, 0) = (float)testY;
			CV_MAT_ELEM(*testPosition, float, 2, 0) = 1;
			
			cvMatMul(homography, testPosition, testResult);
	
			resultX = (int)(CV_MAT_ELEM(*testResult, float, 0, 0) / CV_MAT_ELEM(*testResult, float, 2, 0));
			resultY = (int)(CV_MAT_ELEM(*testResult, float, 1, 0) / CV_MAT_ELEM(*testResult, float, 2, 0));
			
			rightFeature.x = resultX;
			rightFeature.y = resultY;
			
			
			testPair.left = leftFeature;
			testPair.right = rightFeature;
			
			cvSeqPush(testResults, &testPair);
			
			//Debug::message("Homography test: (%d %d) -> (%d %d)", Debug::DEBUG, testX, testY, resultX, resultY);
			
		}
		for (int i = 0; i < pairs->total; ++i) {
			
			
			leftFeature.x = ((FeatureDetector::FeaturePair*)(cvGetSeqElem(pairs, i)))->left.x;
			leftFeature.y = ((FeatureDetector::FeaturePair*)(cvGetSeqElem(pairs, i)))->left.y;
			
			CV_MAT_ELEM(*testPosition, float, 0, 0) = leftFeature.x;
			CV_MAT_ELEM(*testPosition, float, 1, 0) = leftFeature.y;
			CV_MAT_ELEM(*testPosition, float, 2, 0) = 1;
			
			cvMatMul(homography, testPosition, testResult);
	
			resultX = (int)(CV_MAT_ELEM(*testResult, float, 0, 0) / CV_MAT_ELEM(*testResult, float, 2, 0));
			resultY = (int)(CV_MAT_ELEM(*testResult, float, 1, 0) / CV_MAT_ELEM(*testResult, float, 2, 0));
			
			rightFeature.x = resultX;
			rightFeature.y = resultY;
			
			testPair.left = leftFeature;
			testPair.right = rightFeature;
			
			cvSeqPush(testResults, &testPair);
		}
		
		this->gui->showCorrelation(image1, image2, testResults, "Computed_homography", leftIndex, rightIndex);
	
	/* ================================================================ */
	/* ====================== END HOMOGRAPHY TEST ===================== */
	/* ================================================================ */
	
	
}




void MaKI::findFundamentalMatrix(CvSeq* featurePairs){


	int pointCount = featurePairs->total;
	
	if (pointCount > 7) {
	
		CvMat* points1;
		CvMat* points2;
		//CvMat* status;
		CvMat* fundamentalMatrix;
		
		points1 = cvCreateMat(2,pointCount,CV_32FC1);
		points2 = cvCreateMat(2,pointCount,CV_32FC1);
		//status = cvCreateMat(1,pointCount,CV_8UC1);
	
		FeatureDetector::FeaturePair* currentFeature;
		
		
		for(int i = 0; i < pointCount; ++i) {
			
			currentFeature = (FeatureDetector::FeaturePair*)cvGetSeqElem(featurePairs, i);
			
		    /*points1->data.db[i*2]   = (float)(currentFeature->left.x);
		    points1->data.db[i*2+1] = (float)(currentFeature->left.y);
		    points2->data.db[i*2]   = (float)(currentFeature->right.x);
		    points2->data.db[i*2+1] = (float)(currentFeature->right.y);*/
			cvmSet(points1, 0, i, (float)(currentFeature->left.x));
			cvmSet(points1, 1, i, (float)(currentFeature->left.y));
			cvmSet(points2, 0, i, (float)(currentFeature->right.x));
			cvmSet(points2, 1, i, (float)(currentFeature->right.y));
		}
	
		fundamentalMatrix = cvCreateMat(3,3,CV_32FC1);
		int fm_count = cvFindFundamentalMat( points1,points2,fundamentalMatrix, CV_FM_RANSAC,1.0,0.99,NULL );
	
		if (fm_count == 1){
			Output::message("Found fundamental matrix: ", Output::DEBUG);
			
			for (int i = 0; i < 3; ++i) {
				
				Output::message("%f %f %f", Output::DEBUG, cvmGet(fundamentalMatrix, i, 0), cvmGet(fundamentalMatrix, i, 1), cvmGet(fundamentalMatrix, i, 2));
			}
			
			/*FeatureDetector::FeaturePair* testPair = (FeatureDetector::FeaturePair*)cvGetSeqElem(featurePairs, 0);
			CvPoint3D64f leftPoint = cvPoint3D64f(testPair->left.x, testPair->left.y, 1.0);
			CvPoint3D64f rightPoint = cvPoint3D64f(testPair->right.x, testPair->right.y, 1.0);
			CvPoint3D64f epiLine;
			double result;
			
			Debug::message("Points: (%f, %f, %f) -> (%f, %f, %f)", Debug::DEBUG, leftPoint.x, leftPoint.y, leftPoint.z, rightPoint.x, rightPoint.y, rightPoint.z);
			
			epiLine.x = cvmGet(fundamentalMatrix, 0, 0) * leftPoint.x + cvmGet(fundamentalMatrix, 0, 1) * leftPoint.y + cvmGet(fundamentalMatrix, 0, 2) * leftPoint.z;
			epiLine.y = cvmGet(fundamentalMatrix, 1, 0) * leftPoint.x + cvmGet(fundamentalMatrix, 1, 1) * leftPoint.y + cvmGet(fundamentalMatrix, 1, 2) * leftPoint.z;
			epiLine.z = cvmGet(fundamentalMatrix, 2, 0) * leftPoint.x + cvmGet(fundamentalMatrix, 2, 1) * leftPoint.y + cvmGet(fundamentalMatrix, 2, 2) * leftPoint.z;
			
			Debug::message("EpiLine: (%f, %f, %f)", Debug::DEBUG, epiLine.x, epiLine.y, epiLine.z);
			
			result = rightPoint.x * epiLine.x + rightPoint.y * epiLine.y + rightPoint.z * epiLine.z;
			
			Debug::message("Result: %f", Debug::DEBUG, result);*/
			
			CvMat* epilines = cvCreateMat(3,pointCount,CV_32FC1);
			
			cvComputeCorrespondEpilines(points2, 1, fundamentalMatrix, epilines);
			
			/* ===== test ===== */
			double result;
			FeatureDetector::FeaturePair* testPair = (FeatureDetector::FeaturePair*)cvGetSeqElem(featurePairs, 5);
			//CvPoint3D64f rightPoint = cvPoint3D64f(testPair->right.x, testPair->right.y, 1.0);
			CvPoint3D64f rightPoint = cvPoint3D64f(testPair->left.x, testPair->left.y, 1.0);
			
			result = cvmGet(epilines, 0, 5) * rightPoint.x + cvmGet(epilines, 1, 5) * rightPoint.y + cvmGet(epilines, 2, 5);
			Output::message("Result: %f", Output::DEBUG, result);
			/* === end test === */
			
		} else {
			Output::message("Could not find fundamental matrix!", Output::CRITICAL);
		}
		
	} else {
		Output::message("Too few points to find fundamental matrix! (Min = 8)", Output::CRITICAL);
	}
	
	
	
}


void MaKI::adjustPairIDs(CvSeq* featurePairs, int leftImageID, int rightImageID){
	
	Output::message("Setting IDs for images %d and %d", Output::DEBUG, leftImageID, rightImageID);
	
	uint64_t baseID;
	uint64_t id;
	
	baseID = (uint64_t)leftImageID * 10000000ll + (uint64_t)rightImageID * 10000ll;
	for (int i = 0; i < featurePairs->total; ++i) {
		id = baseID + ((FeatureDetector::FeaturePair*)(cvGetSeqElem(featurePairs, i)))->id;
		((FeatureDetector::FeaturePair*)(cvGetSeqElem(featurePairs, i)))->id = id;
		((FeatureDetector::FeaturePair*)(cvGetSeqElem(featurePairs, i)))->leftImageID = leftImageID;
		((FeatureDetector::FeaturePair*)(cvGetSeqElem(featurePairs, i)))->rightImageID = rightImageID;
		//Output::message("Id for pair %d: %010llu", Output::DEBUG, i, id);
	}
	
}




